#include "SimpleVectorMatrix.h"

#include <cmath>


double distance(Point &p1, Point &p2)
{
	return sqrt((p1.x-p2.x)*(p1.x-p2.x) + (p1.y-p2.y)*(p1.y-p2.y));
}

Point distance_v(Point &p1, Point &p2)
{
	Point result;
	result.x = p1.x-p2.x;
	result.y = p1.y-p2.y;
	return result;
}

Point3D distance_v(Point3D &p1, Point3D &p2)
{
	Point3D result;
	result.x = p1.x-p2.x;
	result.y = p1.y-p2.y;
	result.z = p1.y-p2.z;
	return result;
}

void average(Point &p1, Point &p2, int n)
{
	p1.x = (p1.x*n + p2.x)/(n+1);
	p1.y = (p1.y*n + p2.y)/(n+1);
}

void correct(Point3D &p1, Point3D &p2, int n)
{
	p1.x = p1.x + p2.x / (n+1);
	p1.y = p1.y + p2.y / (n+1);
	p1.z = p1.z + p2.z / (n+1);
}

void transposeMatrix16(float* m)
{
	float tm[16];

	tm[ 0] = m[ 0];
	tm[ 1] = m[ 4];
	tm[ 2] = m[ 8];
	tm[ 3] = m[12];
	tm[ 4] = m[ 1];
	tm[ 5] = m[ 5];
	tm[ 6] = m[ 9];
	tm[ 7] = m[13];
	tm[ 8] = m[ 2];
	tm[ 9] = m[ 6];
	tm[10] = m[10];
	tm[11] = m[14];
	tm[12] = m[ 3];
	tm[13] = m[ 7];
	tm[14] = m[11];
	tm[15] = m[15];

	for (int i=0; i<16; ++i)
		m[i] = tm[i];
}

void transposeMatrix9(float* m)
{
	float tm[9];

	tm[0] = m[0];
	tm[1] = m[3];
	tm[2] = m[6];

	tm[3] = m[1];
	tm[4] = m[4];
	tm[5] = m[7];

	tm[6] = m[2];
	tm[7] = m[5];
	tm[8] = m[8];

	for (int i=0; i<9; ++i)
		m[i] = tm[i];
}

float dot(float* v1, float* v2, int size)
{
	float sum = 0;
	for (int i=0; i<size; ++i)
		sum += v1[i]*v2[i];
	return sum;
}

float length(float* v, int size)
{
	return sqrt(dot(v, v, size));
}

float angleBetweenVectors2(float* v1, float* v2)
{
	return acos(dot(v1,v2,2)/(length(v1,2)*length(v2,2)));
}

void mult9(float *A, float *B, float *C)
{
	for (int i=0; i<3; ++i)
	{
		for (int j=0; j<3; ++j)
		{
			C[i*3+j] = 0;
			for (int k=0; k<3; ++k)
				C[i*3+j] += A[i*3+k]*B[k*3+j];
		}
	}
}


//  0  1  2  3
//  4  5  6  7
//  8  9 10 11
// 12 13 14 15

void matrix2axis_angle4(float* pose_matrix, float &alpha, float &x, float &y, float &z)
{
	//alpha = acos(0.5*(pose_matrix[10] - 1 + sqrt((pose_matrix[10]+1)*(pose_matrix[10]+1) - (pose_matrix[4]-pose_matrix[1])*(pose_matrix[4]-pose_matrix[1]))));

	alpha = acos(0.5*(pose_matrix[0]+pose_matrix[5]+pose_matrix[10]-1));
	float t = 2*sin(alpha);

	x = (pose_matrix[9]-pose_matrix[6])/t;
	y = (pose_matrix[2]-pose_matrix[8])/t;
	z = (pose_matrix[4]-pose_matrix[1])/t;
}

void axis_angle2matrix4(float alpha, float x, float y, float z, float* pose_matrix)
{
	float n = sqrt(x*x + y*y + z*z);
	x = x / n;
	y = y / n;
	z = z / n;

	float c = cos(alpha);
	float s = sin(alpha);

	pose_matrix[ 0] = (1-c)*x*x +   c;
	pose_matrix[ 1] = (1-c)*x*y - z*s;
	pose_matrix[ 2] = (1-c)*x*z + y*s;

	pose_matrix[ 4] = (1-c)*x*y + z*s;
	pose_matrix[ 5] = (1-c)*y*y +   c;
	pose_matrix[ 6] = (1-c)*y*z - x*s;

	pose_matrix[ 8] = (1-c)*x*z - y*s;
	pose_matrix[ 9] = (1-c)*y*z + x*s;
	pose_matrix[10] = (1-c)*z*z + c;
}

//void rotate_matrix(float* pose_matrix, float angle)
//{
//	float a, x, y, z;
//
//	matrix2axis_angle4(pose_matrix, a, x, y, z);
//	axis_angle2matrix4(a+angle, x, y, z, pose_matrix);
//}

void rotate_matrix(float* pose_matrix, float angle)
{
	float pose[9] = {pose_matrix[0], pose_matrix[1], pose_matrix[2],
					 pose_matrix[4], pose_matrix[5], pose_matrix[6],
					 pose_matrix[8], pose_matrix[9], pose_matrix[10]};

	float R[9] = {cos(angle),  -sin(angle),  0,
				  sin(angle),   cos(angle),  0,
				  0,            0,  1};

	float t[9];

	mult9(pose, R, t);

	pose_matrix[ 0] = t[0];
	pose_matrix[ 1] = t[1];
	pose_matrix[ 2] = t[2];

	pose_matrix[ 4] = t[3];
	pose_matrix[ 5] = t[4];
	pose_matrix[ 6] = t[5];

	pose_matrix[ 8] = t[6];
	pose_matrix[ 9] = t[7];
	pose_matrix[10] = t[8];
}


void mult_mat_vector4(double* M, double* v, double* r)
{
	r[0] = M[0]*v[0] + M[1]*v[1] + M[2]*v[2] + M[3]*v[3];
	r[1] = M[4]*v[0] + M[5]*v[1] + M[6]*v[2] + M[7]*v[3];
	r[2] = M[8]*v[0] + M[9]*v[1] + M[10]*v[2] + M[11]*v[3];
	r[3] = M[12]*v[0] + M[13]*v[1] + M[14]*v[2] + M[15]*v[3];
}














